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Figure  1:  To  find  open  spaces  on  a  ID  line  big  enough  to  hold  a  robot  of  size  r,  it  is  sufficient  to 
sample  the  line  at  intervals  of  r. 

Figure  2:  On  a  4-connectcd  2D  grid  of  size  r.  it  is  possible  to  miss  diagonal  paths.  Here,  there  is 
enough  room  for  the  robot  to  go  from  (1,1)  to  (2,2).  blit  there  is  no  clear  path  on  the  grid. 

Figure  3:  One  fix  for  the  problem  in  figure  2  is  to  shrink  the  grid  by  a  factor  of  sqrt(2).  Then,  if  the 
robot  will  fit,  there  will  be  a  grid  path.  Here  the  robot  could  go  from  (1.1)  to  (2.1)  to  (2,2) 
to  (3,2)  and  on. 

Figured:  Another  fix  is  to  use  an  8-counectcd  grid.  Now  there  is  a  diagonal  line  directly 
connecting  (1,1)  with  (2,2). 


Abstract 


Path  Relaxation  is  a  method  of  planning  sale  paths  around  obstacles  for  mobile  robots.  It  works  in  two  steps: 
a  global  grid  search  that  finds  a  rough  path,  followed  by  a  local  relaxation  step  that  adjusts  each  node  on  the 
Path  to  lower  the  overall  path  cost.  The  representation  used  by  Path  Relaxation  allows  an  explicit  tradeoff 
among  length  of  path,  clearance  away  from  obstacles,  and  distance  traveled  through  unmapped  areas. 


I 


1.  Introduction 

Path  Relaxation  is  a  two-step  process  for  mobile  robots.  It  finds  a  safe  path  for  a  robot  to  traverse  a  field  of 
obstacles  and  arrive  at  its  destination.  The  first  step  of  path  relaxation  finds  a  preliminary  path  on  an 
eight-connected  grid  of  points.  The  second  step  adjusts,  or  "relaxes",  the  position  of  each  preliminary  pa  Lit 
point  to  improve  the  path. 

One  advantage  of  path  relaxation  is  that  it  allows  many  dilTcrcnl  factors  to  be  considered  in  choosing  a  path. 
Typical  path  planning  algorithms  evaluate  the  cost  of  alternative  paths  solely  on  the  basis  of  path  length.  The 
cost  function  used  by  Path  Relaxation,  in  contrast,  also  includes  how  close  the  path  comes  to  objects  (the 
further  away,  the  lower  die  cost)  and  penalties  for  traveling  through  areas  out  of  the  Held  of  view.  The  effect 
is  to  produce  paths  that  neither  clip  the  corners  of  obstacles  nor  make  wide  deviations  around  isolated  objects, 
and  that  prefer  to  stay  in  mapped  terrain  unless  a  path  through  unmapped  regions  is  substantially  shorter. 
Other  factors,  such  as  sharpness  of  corners  or  visibility  of  landmarks,  could  also  be  added  for  a  particular 
robot  or  mission. 

Path  Relaxation  is  part  of  Fido,  the  vision  and  navigation  system  of  the  CMU  Rover  mobile  robot.  [29. 41] 
The  Rover,  under  l  ido’s  control,  will  navigate  solely  by  stereo  vision.  It  will  pick  about  40  points  to  track, 
find  them  in  a  pair  of  stereo  images,  and  calculate  their  31)  positions  relative  to  the  Rover.  The  Rover  will 
then  move  about  half  a  meter,  take  a  new  pair  of  pictures,  find  the  40  tracked  points  in  each  of  the  new 
pictures  and  recalculate  dicir  positions.  The  change  in  position  of  those  points  relative  to  the  robot  gives  the 
actual  change  of  the  robot's  position  in  the  stationary  world. 

Fido's  model  of  die  world  is  not  suitable  for  most  existing  path-planning  algorithms.  Algorithms  for 
planning  paths  usually  assume  a  completely  known  world  model  composed  of  planar-faced  objects.  Fido's 
world  model,  on  the  other  hand,  contains  only  the  40  points  it  is  tracking.  Tor  each  point  the  model  records 
its  position,  the  uncertainty  in  that  position,  and  the  appearance  of  a  small  patch  of  the  image  around  that 
point.  Furthermore,  Kido  only  knows  about  what  it  has  seen:  points  that  have  never  been  within  its  field  of 
view  arc  not  listed  in  the  world  model.  Also,  the  vision  system  may  fail  to  track  points  correctly,  so  there  may 
be  phantom  objects  in  die  world  model  that  have  been  seen  once  but  arc  tut  longer  being  tracked.  All  this 
indicates  the  need  for  a  data  structure  that  can  represent  uncertainty  and  inaccuracy,  and  for  algoridims  that 
can  use  such  data. 

Section  2  of  diis  report  outlines  die  constraints  available  to  Tido's  path  planner.  Section  3  discusses  some 
common  types  of  path  planners,  and  shows  how  dtey  arc  inadequate  for  our  application.  The  Path  Relaxation 
algorithm  is  explained  in  detail  in  Section  4,  and  some  additions  to  the  basic  scheme  are  presented  in  Section 
5.  Finally,  Section  6  discusses  shortcomings  of  Path  Relaxation  and  some  possible  extensions. 


2.  Constraints 


An  intelligent  path  planner  needs  to  bring  much  information  to  bear  on  the  problem.  This  section  discusses 
some  of  the  information  useful  for  mobile  robot  path  planning,  and  shows  how  the  constraints  for  mobile 
robot  paths  differ  from  diosc  for  manipulator  trajectories. 

Low  dimensionality.  A  ground-based  robot  vehicle  is  constrained  to  three  degrees  of  freedom:  x  and  y 
position  and  orientation.  In  particular,  the  CMU  Rover  has  a  circular  cross-section,  so  for  the  purposes  of 
path  planning  the  orientation  docs  not  matter.  This  makes  path  planning  only  a  two-dimensional  problem. 
In  contrast,  typical  robot  turns  have  from  3  to  6  joints.  A  path  planner  for  a  manipulator,  if  it  lakes  into 
account  the  position  of  each  joint,  would  have  up  to  a  6-dimensional  search  space. 

Imprecise  control.  Kven  under  the  best  of  circumstances,  a  mobile  robot  is  not  likely  to  be  very  accurate.  Its 
precision  will  depend  on  the  smoothness  of  the  ground,  the  accuracy  of  its  controller,  and  the  traction  of  the 
wheels.  Typical  manipulators,  on  the  other  hand,  have  repeatabilities  of  a  few  thousandths  of  an  inch  over 
their  entire  reach.  The  implication  for  path  planning  is  that  it  is  much  less  important  to  worry  about  exact  Ills 
for  mobile  robot  paths.  If  the  robot  could,  theoretically,  just  barely  lit  through  a  certain  opening,  then  in 
practice  that's  probably  not  a  good  way  to  go.  Computational  resources  are  better  spent  exploring  alternate 
paths  rather  than  worrying  about  highly  accurate  motion  calculations. 

Cumulative  error.  l-Trors  in  a  dead-reckoning  system  tend  to  accumulate:  a  small  error  in  heading,  for 
instance,  can  give  rise  to  a  large  error  in  position  as  the  vehicle  moves.  The  only  way  to  reduce  error  is  to 
periodically  measure  position  against  some  global  standard,  which  can  be  time-consuming.  The  Rover,  for 
example,  docs  its  measurement  by  stereo  vision,  taking  a  few  minutes  to  compute  its  exact  position.  So  a 
slightly  longer  path  that  stays  farther  away  from  obstacles,  and  allows  longer  motion  between  stops  for 
measurement,  may  take  less  time  to  travel  than  a  shorter  path  that  requires  more  frequent  slops.  In  contrast,  a 
manipulator  can  reach  a  location  with  approximately  the  same  error  regardless  of  what  path  is  taken  to  arrive 
there.  There  is  no  cumulative  error,  and  no  time  spent  in  reorientation. 

Unknown  areas.  Robot  manipulator  trajectory  planners  usually  know  about  all  the  obstacles.  The  Rover 
knows  only  about  those  that  it  has  seen.  This  leaves  unknown  areas  outside  its  field  of  view  and  behind 
obstacles.  It  is  usually  preferable  to  plan  a  path  that  traverses  only  known  empty  regions,  but  if  that  path  is 
much  longer  than  the  shortest  possible  path  then  it  may  be  worth  while  looking  at  the  unknown  regions. 
Perhaps  some  "curiosity  factor",  that  changed  depending  on  whether  this  was  a  mapping  run  or  a  production 
run.  could  determine  the  tendency  to  look  around. 

Fuzzy  objects.  Not  only  do  typical  manipulator  path-planners  know  about  all  the  objects,  they  know 
precisely  where  each  object  is.  This  information  might  conic,  for  instance,  from  the  CAD  system  that 
designed  the  robot  workstation.  Mobile  robots,  on  the  other  hand,  usually  sense  the  world  as  they  go.  The 
Rover,  instead  of  having  precise  bounds  for  objects,  knows  only  about  fuzzy  points.  The  location  of  a  point  is 
only  known  to  the  precision  of  the  stereo  vision  system,  and  the  extent  of  an  object  beyond  the  point  is 
entirely  unknown. 


Mdd  of  view.  It  may  be  important  for  a  mobile  robot  to  plan  paths  ihat  keep  it  as  far  away  from  obstacles  as 
possible,  so  that  it  can  see  more  distant  objects.  Or  it  may  be  important  to  keep  it  behind  objects,  so  that  it 
cannot  be  seen  from  far  off:  imagine  a  robot  watchman,  ora  robot  lank,  sneaking  up  on  some  hostile  force. 

In  summary,  a  good  system  for  mobile  robot  path  planning  will  be  quite  different  from  a  manipulator  path 
planner.  Mobile  robot  path  planners  need  to  handle  uncertainty  in  the  sensed  world  model  and  errors  in  path 
execution.  They  do  not  have  to  worry  about  high  dimensionality  or  extremely  high  accuracy.  Section  3  of 
this  report  discusses  some  existing  path  planning  algorithms  and  their  shortcomings.  Section  4  then  presents 
the  algorithms  used  by  Path  Relaxation,  and  shows  how  they  address  these  problems. 


3.  Approaches  to  Path  Planning 


This  section  outlines  several  approaches  to  path  planning  and  some  of  the  drawbacks  of  each  approach.  All 
of  these  methods  except  the  potential  fields  approach  abstract  the  search  space  to  a  graph  of  possible  paths. 
This  graph  is  then  searched  by  some  standard  search  technique,  such  as  breadth-first  or  A*  [30.  38,  39|.  and 
the  shortest  path  is  returned.  The  important  thing  to  note  in  the  following  is  the  information  made  explicit  by 
each  representation  and  the  information  thrown  away. 

f  ree  Space  methods.  [3.  13,  31,  32j  One  type  of  path  planner  explicitly  deals  with  the  free  space  rather  than 
with  die  space  occupied  by  obstacles,  and  forces  path  segments  to  run  down  the  middle  of  the  corridors 
between  obstacles.  One  such  method  fits  generalized  cylinders  to  die  free  space.  Another  calculates  the  free 
space’s  Voronoi  diagram.  The  spine  of  the  Voronoi  diagram  or  the  axes  of  the  generalized  cylinders  form  a 
network  of  possible  paths.  Some  of  these  paths  may  pass  through  places  narrower  than  the  robot:  these  path 
segments  can  be  detected  and  deleted.  The  remaining  segments  form  the  graph  of  possible  paths  which  is 
searched  to  find  the  shortest  path. 

Free  space  algorithms  suffer  from  two  related  problems,  both  resulting  from  a  data  abstraction  that  throws 
away  too  much  information.  The  first  problem  is  that  paths  always  run  down  the  middle  of  corridors.  In  a 
narrow  space,  this  is  desirable,  since  it  allows  the  maximum  possible  robot  error  without  hitting  an  object. 
Hut  in  some  cases  paths  may  go  much  further  out  of  their  way  than  necessary.  The  second  problem  is  that  the 
algorithms  do  not  use  clearance  information.  The  shortest  path  is  always  selected,  even  if  it  involves  much 
closer  tolerances  dian  a  slightly  longer  path. 

Vertex  Graphs.  [24,  40,  28]  Another  class  of  algorithms  is  based  on  a  graph  connecting  pairs  of  vertices.  In 
the  first  step,  each  obstacle  is  expanded  by  the  size  of  the  robot  and  the  robot  conceptually  shrunk  to  a  point. 
The  problem  of  finding  a  path  for  the  point  through  the  grown  obstacles  is  exactly  the  same  as  finding  a  path 
for  the  whole  robot  through  die  original  objects.  The  graph  of  possible  paths  is  built  by  considering  every 
pair  of  vertices  of  the  expanded  obstacles:  if  the  line  between  two  vertices  docs  not  intersect  any  of  the 
expanded  obstacles,  it  is  a  candidate  padi  segment  and  is  added  to  the  graph.  As  in  the  Free  Space  methods, 
the  graph  is  searched  by  some  standard  graph  search  algorithm,  and  the  shortest  path  is  returned.  Variations 
of  this  algorithm  use  schemes  that  interleave  generation  and  testing  of  the  graph,  hoping  to  avoid  building 
parts  of  the  graph. 

Vertex  graph  algorithms  suffer  from  the  "too  close”  problem:  in  their  concern  for  the  shortest  possible  path, 
they  find  paths  that  clip  the  corners  of  obstacles  and  even  run  along  the  edges  of  some  objects.  It  is,  of  course, 
possible  to  build  in  a  margin  of  error  by  growing  die  obstacles  by  an  extra  amount:  this  may,  however,  block 
some  paths. 

both  free  space  and  vertex  graph  methods  throw  away  too  much  information  too  soon.  All  obstacles  arc 
modeled  as  polygons,  all  paths  are  considered  cither  open  or  blocked,  and  the  shortest  path  is  always  best. 
There  is  no  mechanism  for  trading  a  slightly  longer  padi  for  more  clearance,  or  for  making  local  path 
adjustments.  There  is  also  no  clean  way  to  deal  with  unmapped  regions,  other  than  to  close  them  off  entirely. 


The  Potential  Fields  [1,  20J  approach  tries  to  make  those  tradeoffs  explicit.  Conceptually,  it  turns  the  robot 
into  a  marble,  tilts  the  floor  towards  the  goal,  and  watches  to  sec  which  way  the  marble  rolls.  Obstacles  arc 
represented  as  hills  with  sloping  sides,  so  the  marble  will  roll  a  prudent  distance  away  from  them  but  not  loo 
far,  and  will  seek  the  passes  between  adjacent  hills.  Sophisticated  algorithms  even  give  die  marble 
momentum,  taking  into  account  the  energy  needed  to  accelerate,  decelerate,  and  turn.  The  problem  with 
potential  field  paths  is  that  they  tend  to  get  caught  in  dead  ends:  once  the  marble  rolls  into  a  box  canyon,  the 
algorithm  has  to  invoke  special-case  mechanisms  to  cut  off  that  route,  backtrack,  and  start  again.  Moreover, 
the  path  with  the  lowest  threshold  might  turn  out  to  be  a  long  and  winding  road,  while  a  path  that  limb 
a  small  ridge  at  the  start  and  then  has  an  easy  run  to  the  goal  might  never  be  investigated.  So  of  these 
problems  can  be  avoided  if  there  arc  no  concave  objects  or  collections  of  objects,  but  this  is  ;  -<mg  and 
unrealistic  restriction. 

Another  approach  that  could  explicitly  represent  the  conflicts  between  short  paths  and  obstacle  avoidance  is 
the  Regular  Grid  method.  This  covers  the  world  with  a  regular  grid  of  points,  each  connected  with  its  4  or  8 
neighbors  to  form  a  graph.  In  existing  regular  grid  implementations,  the  only  information  stored  at  a  node  is 
whether  it  is  inside  an  object  or  not.  Then  the  graph  is  searched,  and  the  shortest  grid  path  returned.  This 
straightforward  grid  search  has  many  of  the  same  “too  close"  problems  as  the  vertex  graph  approaches.  A 
more  sophisticated  approach  could  assign  weights  to  die  nodes  depending  on  how  close  they  were  to  objects 
or  other  factors,  and  avoid  the  "too  close"  and  "too  far"  problems.  Hut  the  remaining  path  would  still  be 
jagged,  and  might  miss  the  best  path  because  of  die  grid’s  coarse  resolution. 
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4.  Path  Relaxation 

Path  Relaxation  combines  the  best  features  of  grid  search  and  potential  fields.  Using  the  rolling  marble 
analogy,  the  first  step  is  a  global  grid  search  that  finds  a  good  valley  for  the  path  to  follow.  The  second  step  is 
a  local  relaxation  step,  similar  to  the  potential  Held  approach,  that  moves  the  nodes  in  the  path  to  the  bottom 
of  the  valley  in  which  they  lie.  The  terrain  (cost  function)  consists  of  a  gradual  slope  towards  the  goal,  hills 
with  sloping  sides  for  obstacles,  and  plateaus  for  unexplored  regions.  The  height  of  the  hills  has  to  do  with 
the  confidence  that  there  really  is  an  object  there.  Hill  diameter  depends  on  robot  precision:  a  more  precise 
robot  can  drive  closer  to  tin  object,  so  the  hills  will  bo  tall  and  narrow,  while  a  less  accurate  vehicle  will  need 
more  clearance,  requiring  wide,  gradually  tapering  hillsides. 

This  section  first  presents  results  on  how  large  the  grid  si/e  can  be  without  missing  paths.  It  next  discusses 
the  mechanism  for  assigning  cost  to  the  nodes  and  searching  the  grid.  Finally,  it  presents  the  relaxation  step 
that  adjusts  the  positions  of  path  nodes. 

4.1  Grid  Size 

How  large  can  a  grid  be  and  still  not  miss  any  possible  paths?  That  depends  on  the  number  of  dimensions 
of  the  problem,  on  the  connectivity  of  the  grid,  and  on  the  size  of  the  vehicle.  It  also  depends  on  the  vehicle's 
shape:  in  this  section,  we  discuss  the  simplest  shape,  which  is  a  vehicle  with  a  circular  cross-section. 


Figure  l:  To  find  open  spaces  on  a  11)  line  big  enough  to  hold  a  robot  of  si/c 
r,  it  is  sufficient  to  sample  the  line  at  intervals  of  r. 

First  consider  the  problem  of  placing  a  circle  on  all)  line  (sec  figure  I).  Given  a  line  segment  with  some 
sections  covered  with  II)  obstacles,  and  a  robot  of  size  r,  we  wish  to  find  all  clear  places  on  the  line  where  the 
robot  can  be  placed.  It  is  sufficient  in  this  case  to  sample  the  segment  at  intervals  of  r  and  test  only  the 
unblocked  points.  A  II)  grid  spacing  of  r guarantees  that  any  open  section  of  length  cor  greater  will  touch  at 
least  one  of  the  grid  points.  An  open  point  does  not  guarantee  that  it  is  pari  of  an  open  interval  of  size  /•;  it 
merely  says  that  point  is  worth  investigating. 

We  can  extend  this  reasoning  from  one  dimension  to  a  more  useful  case,  a  21)  area  with  21)  obstacles.  The 
area  can  be  covered  with  a  grid  in  which  each  node  is  connected  to  either  its  four  or  its  eight  nearest 
neighbors.  For  a  four-connected  grid,  if  the  spacing  were  /,  there  would  be  a  chance  of  missing  diagonal 
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Figure  4:  Another  fix  is  to  use  an  8-conncctcd  grid.  Now  there  is  a 
diagonal  line  directly  connecting  (1,1)  with  (2.2). 

paths:  there  might  be  just  enough  room  between  two  obstacles  for  the  robot  to  move  front  (x.  y)  to 
(x  +  I,  y+ 1),  yet  both  node  (x,  y+  l)  and  node  (x  + 1.  y)  might  be  covered.  To  guarantee  that  no  paths  arc 
missed,  the  grid  spacing  must  be  reduced  to  rV 2  /2.  That  is  the  largest  size  allowable  that  guarantees  that  if. 
diagonally  opposite  nodes  arc  covered,  there  is  not  enough  room  between  them  for  the  robot  to  safely  pass. 

If  the  grid  is  eight-connected  (each  node  connected  to  its  diagonal,  as  well  as  orthogonal,  neighbors),  die 
problem  with  diagonal  paths  disappears.  As  in  the  II)  case,  the  grid  spacing  can  be  a  full  /;  while 
guaranteeing  that  if  there  is  a  path  it  will  be  found. 


4.2  Grid  Search 


y. 


Once  the  grid  size  has  been  fixed,  the  next  step  is  to  assign  costs  to  paths  on  the  grid  and  then  to  search  for 
the  best  path  along  the  grid  from  the  start  to  the  goal.  "Best",  in  this  case,  has  three  conflicting  requirements: 
shorter  path  length,  greater  margin  away  from  obstacles,  and  less  distance  in  uncharted  areas.  These  three  arc 
explicitly  balanced  by  the  way  path  costs  arc  calculated.  A  path's  cost  is  the  sum  of  the  costs  of  the  nodes 
through  which  it  passes,  each  multiplied  by  the  distance  to  the  adjacent  nodes.  (In  a  4-connected  graph  all 
lengths  arc  the  same,  but  in  an  8-conncctcd  graph  we  have  to  distinguish  between  orthogonal  and  diagonal 
links.)  The  node  costs  consist  of  three  parts  to  explicitly  represent  the  three  conflicting  criteria. 


1.  Cost  for  distance.  Kach  node  starts  out  with  a  cost  of  one  unit,  for  length  traveled. 


2.  Cost  for  near  objects.  Kach  object  near  a  node  adds  to  that  node's  cost.  The  nearer  the  obstacle, 
the  more  cost  it  adds.  'The  exact  slope  of  the  cost  function  will  depend  on  the  accuracy  of  the 
vehicle  (a  more  accurate  vehicle  can  afford  to  come  closer  to  objects),  and  the  vehicle’s  speed  (a 
faster  vehicle  can  afford  to  go  farther  out  of  its  way),  among  other  factors. 


* 


9 


3.  Cost  for  within  or  near  an  unmapped  region.  The  cost  for  traveling  in  an  unmapped  region  will 
depend  on  the  vehicle’s  mission.  If  this  is  primarily  an  exploration  trip,  for  example,  the  cost 
might  be  relatively  low.  There  is  also  a  cost  added  for  being  near  an  unmapped  region,  using  the 
same  sort  of  function  of  distance  as  is  used  for  obstacles.  I  bis  provides  a  buffer  to  keep  paths 
from  coining  too  close  to  potentially  unmapped  hazards. 

The  first  step  of  Path  Relaxation  is  to  set  up  the  grid  and  construct  the  list  of  obstacles  and  the  vehicle's 
current  position  and  Held  of  view.1  The  system  can  then  calculate  the  cost  at  each  node,  based  on  the 
distances  to  nearby  obstacles  and  whether  that  node  is  within  the  Held  of  view  or  not.  The  next  step  is  to 
create  links  from  each  node  to  its  8  neighbors.  The  start  and  goal  locations  do  not  necessarily  lie  on  grid 
points,  so  special  nodes  need  to  be  created  for  them  and  linked  into  the  graph. 

The  system  then  searches  this  graph  for  the  minimum-cost  path  from  the  start  to  the  goal.  The  search  itself 
is  a  standard  A*  [30]  search.  The  estimated  total  cost  of  a  path,  used  by  A*  to  pick  which  node  to  expand 
next,  is  the  sum  of  die  cost  so  far  plus  the  straight-line  distance  from  the  current  location  to  the  goal.  This  has 
the  effect,  in  regions  of  equal  cost,  of  finding  the  padi  that  most  closely  approximates  the  straight-line  paLh  to 
the  goal. 

The  padi  found  is  guaranteed  to  be  the  lowest-cost  path  on  the  grid,  but  this  is  not  necessarily  the  overall 
optimal  path,  hirst  of  all,  even  in  areas  with  no  obstacles  the  grid  path,  may  be  longer  Ilian  a  straight-line  path 
simply  because  it  has  to  follow  grid  lines.  For  a  4-connecicd  grid,  die  worst  case  is  diagonal  lines,  where  the 
grid  path  is  V2~  limes  as  long  as  the  straight-line  path.  For  an  8-coimcctcd  grid,  the  equivalent  worst  case  is 
a  path  that  goes  equal  distances  forward  and  diagonally.  This  gives  a  path  about  1.08  times  as  long  as  the 
straight-line  path.  In  cases  where  die  path  curves  around  several  obstacles,  the  extra  padi  length  can  be  even 
more  significant.  Secondly,  if  the  grid  padi  goes  between  two  obstacles,  it  may  be  lion-optimal  because  a 
node  is  placed  closer  to  one  obstacle  than  to  the  other.  A  node  placed  exactly  half  way  between  the  two 
obstacles  would,  for  most  types  of  cost  functions,  have  a  lower  cost.  The  placement  of  the  node  that 
minimizes  die  overall  path  cost  will  depend  both  on  node  cost  and  on  path  length,  but  in  any  case  is  unlikely 
to  be  exactly  on  a  grid  point.  If  die  grid  path  is  topologically  equivalent  to  the  optimal  padi  (i.c.  goes  on  the 
same  side  of  each  object),  the  grid  padi  can  be  iteratively  improved  to  approximate  the  optimal  path  (sec 
Section  5).  Rut  if  the  grid  padi  at  any  point  goes  on  the  "wrong"  side  of  an  obstacle,  then  no  amount  of  local 
adjustment  will  y  ield  die  optimal  path.  The  chance  of  going  on  the  wrong  side  of  an  obstacle  is  related  to  the 
size  of  the  grid  and  the  shape  of  the  cost  vs.  distance  function.  For  a  given  grid  size  and  cost  function,  it  is 
possible  to  pul  a  limit  on  how  much  worse  the  path  found  could  possibly  be  than  the  optimal  path.  If  the 
result  is  too  imprecise,  the  grid  size  can  be  decreased  until  the  additional  computation  lime  is  no  longer  worth 
the  improved  padi. 

Up  to  this  point,  it  has  been  assumed  that  the  path  is  passable,  that  is  that  there  is  enough  clearance  for  the 
vehicle  at  all  points.  At  this  point  that  assumption  can  be  tested.  If  any  path  link  goes  between  two  objects 


In  this  implementation,  there  arc  two  types  of  obstacles:  polygonal  ami  circular.  Currently,  the  circular  obstacles  arc  used  for  points 
found  by  ! -Ido's  vision  system,  each  bounded  by  a  circular  error  limit,  and  the  polygons  are  used  for  the  field  of  view  The  vision  system 
will  eventually  give  polygonal  obstacles,  at  which  point  both  the  obstacles  aud  the  field  of  view  will  be  icpiesenied  as  polygons  and  the 
circular  obstacles  will  no  longer  be  needed. 


separated  by  less  than  the  robot  diameter,  die  robot  cannot  squeeze  through  that  gap  and  the  path  is  not 
passable.  Hie  link  that  was  crossed  can  be  removed,  and  the  grid  search  redone.  In  practice,  if  the  best  grid 
path  is  not  passable,  it  is  likely  that  no  other  path  will  be,  either.  I  lowever  it  is  possible  for  the  lowest-cost 
path  to  have  a  single  spot  that  is  just  barely  impassable,  while  the  next  lowest  cost  path  has  a  series  of  just 
barely  passable  spots  that  add  tip  to  higher  total  cost.  The  probability  of  this  happening  depends  on  the  shape 
of  the  cost  function,  the  size  of  the  grid,  the  length  of  the  path,  and  the  amount  of  clutter  present. 

A  few  details  on  the  shape  of  the  cost  function  deserve  mention.  Many  different  cost  functions  will  work, 
but  some  shapes  arc  harder  to  handle  properly.  The  first  shape  we  tried  was  linear.  This  had  the  advantage  of 
being  easy  to  calculate  quickly,  but  gave  problems  when  two  objects  were  close  together.  The  sum  of  the  costs 
from  (wo  nearby  objects  was  equal  to  a  linear  function  of  the  sum  of  the  distances  to  the  objects.  This  creates 
ellipses  of  equal  cost,  including  the  degenerate  ellipse  on  the  line  between  the  two  objects.  In  that  case,  there 
was  no  reason  for  the  path  to  pick  a  spot  midway  between  the  objects,  as  wc  had  (incorrectly)  expected. 
Instead,  the  only  change  in  cost  came  from  changing  distance,  so  the  path  went  wherever  it  had  to  to  minimize 
path  length.  In  our  first  attempt  to  remedy  the  situation  wc  replaced  the  linear  slope  with  an  exponentially 
decaying  value.  This  had  die  desired  effect  of  creating  a  saddle  between  the  two  peaks,  and  forcing  the  path 
towards  the  midpoint  between  the  objects.  The  problem  with  exponentials  is  that  they  never  reach  zero,  For 
a  linear  function,  there  was  a  quick  test  to  see  if  a  given  object  was  close  enough  to  a  given  point  to  have  any 
influence.  If  it  wastoo  far  away,  the  function  did  not  have  to  be  evaluated.  For  the  exponential  cost  function, 
on  the  other  hand,  the  cost  function  had  to  be  calculated  for  every  obstacle  for  each  point.  Wc  tried  cutting 
off  the  size  of  the  exponential,  but  this  left  a  small  ridge  at  the  extremum  of  the  function,  and  paths  tended  to 
run  in  nice  circular  arcs  along  those  ridges.  A  good  compromise,  and  the  function  we  finally  settled  on.  is  a 
cubic  function  that  ranges  from  0  at  some  maximum  distance,  set  by  the  user,  to  the  obstacle's  maximum  cost 
at  0  distance.  This  has  both  the  advantages  of  having  a  good  saddle  between  neighboring  obstacles  and  of 
being  easy  to  compute  and  bounded  in  a  local  area. 

4.3  Relaxation 

Grid  search  finds  an  approximate  path;  the  next  step  is  an  optimization  step  that  fine-tunes  the  location  of 
each  node  on  the  path  to  minimize  the  total  cost.  One  way  to  do  this  would  be  to  precisely  define  the  cost  of 
the  path  by  a  set  of  non-linear  equations  and  solve  them  simultaneously  to  analytically  determine  the  optimal 
position  of  each  node,  this  approach  is  not,  in  general,  computationally  feasible.  The  approach  used  here  is  a 
relaxation  method.  Faeli  node's  position  is  adjusted  in  turn,  using  only  local  information  to  minimize  the  cost 
of  the  path  sections  on  cither  side  of  that  node.  Since  moving  one  node  may  affect  the  cost  of  its  neighbors, 
the  entire  procedure  is  repeated  until  no  node  moves  farther  than  some  small  amount. 

Node  motion  has  to  be  restricted.  If  nodes  were  allowed  to  move  in  any  direction,  they  would  all  end  up  at 
low  cost  points,  with  many  nodes  bunched  together  and  a  few  long  links  between  them.  This  would  not  give  a 
very  good  picture  of  the  actual  cost  along  the  path.  So  in  order  to  keep  the  nodes  spread  out.  a  node's  motion 
is  restricted  to  be  perpendicular  to  a  line  between  the  preceding  and  following  nodes.  Furthermore,  at  any 
one  step  a  node  is  allowed  to  move  no  more  than  one  unit. 


As  a  node  moves,  all  three  factors  of  cost  are  affected:  distance  traveled  (from  the  preceding  node,  via  this 


node,  to  die  next  node),  proximity  to  objects,  and  relationship  to  unmapped  regions.  The  combination  of 
diese  factors  makes  it  difficult  to  directly  solve  for  minimum  cost  node  position.  Instead,  a  binary  search  is 
used  to  find  dial  position  to  whatever  accuracy  is  desired. 

The  relaxation  step  has  the  effect  of  turning  jagged  lines  into  straight  ones  where  possible,  of  finding  the 
"saddle"  in  the  cost  function  between  two  objects,  and  of  curving  around  isolated  objects.  It  also  does  die 
"right  thing"  at  region  boundaries.  The  least  cost  path  crossing  a  border  between  different  cost  regions  will 
follow  the  same  padi  as  a  ray  of  light  refracting  at  a  boundary  between  media  with  different  transmission 
velocities.  The  relaxed  path  will  approach  that  path. 


5.  Additions  to  the  Basic  Scheme 


There  arc  several  useful  additions  to  the  basic  path  relaxation  approach.  This  section  describes  those 
additions  that  have  been  implemented  and  tested.  Section  6  describes  other  possible  extensions. 

One  extension  is  to  vary  the  costs  of  individual  obstacles.  The  vision  system  is  not  always  accurate;  it 
sometimes  reports  phantom  objects  that  do  not  really  exist,  and  sometimes  loses  real  objects  that  it  had 
tracked  correctly  for  several  steps.  If  the  path  planner  kept  all  the  objects  the  vision  system  ever  thought  it 
saw,  the  world  map  could  get  hopelessly  jammed.  On  die  other  hand,  if  an  object  were  only  used  for  path 
planning  when  it  was  currently  being  tracked,  the  robot  could  collide  with  objects  that  had  been  picked  up  by 
die  vision  system  at  one  point.  The  solution  is  to  decrease  the  cost  of  objects  dial  are  within  the  field  of  view 
and  should  be  visible,  but  arc  not  reported  by  the  vision  system.  If  the  cost  goes  below  zero,  that  object  gets 
deleted.  Typically,  die  cost  is  decreased  by  10%  of  its  original  value  for  each  turn  it  is  within  the  Held  of  view 
but  not  seen.  So  10  turns  after  the  vision  system  loses  an  object,  it  disappears  from  the  path  planner's  world 
model.  This  is  a  cost-effective,  but  not  very  sophisticated  approach  to  the  problem.  As  the  vision  system 
improves  and  gives  more  reliable  data,  this  will  become  a  more  interesting  problem  and  will  require  more 
work. 

Another  extension  implemented  is  to  re-use  existing  paths  whenever  possible,  l-’ido  invokes  the  path 
planner  at  the  beginning  of  each  step.  ’The  planner  adds  new  points  to  its  world  map.  adjusts  costs  of  existing 
points,  and  calculates  the  current  field  of  view.  It  then  takes  the  previously  calculated  path,  trims  off  nodes 
that  die  vehicle  has  moved  through,  and  splices  in  the  current  position  as  the  new  start  node.  It  checks  this 
path  to  see  if  it  is  still  usable;  each  node  and  each  line  between  nodes  is  cheeked  against  all  obstacles  to  see  if 
it  comes  close  enough  to  cause  a  collision.  If  the  entire  path  is  clear,  it  is  used  again  for  the  next  step.  Only  if 
it  is  blocked  at  some  point  docs  die  path-planner  have  to  start  from  scratch.  A  more  sophisticated  scheme 
would  also  consider  the  chance  diat  a  new,  shorter  padi  had  opened  up  because  of  deleted  obstacles  or  new 
area  in  die  field  of  view.  It  would  then  estimate  the  amount  of  lime  dial  could  be  saved  by  finding  a  new 
path,  and  die  chance  Uiat  a  new  path  could  be  found,  before  it  decided  whether  to  reuse  the  existing  padi  or 
to  replan  from  scratch. 

ITic  relaxation  step  can  be  greatly  speeded  up  if  it  runs  in  parallel  on  several  computers.  Allhough  an  actual 
parallel  implementation  has  not  yet  been  done,  a  simulation  has  been  written  and  tested.  The  major 
difference  is  that  by  the  time  a  node's  position  is  to  be  adjusted  in  the  serial  implementation,  the  previous 
node’s  position  has  already  been  updated,  so  the  adjustments  ripple  down  the  chain  of  nodes.  If  all  nodes  arc 
to  be  adjusted  in  parallel,  though,  a  node’s  position  has  to  updated  based  on  its  predecessor’s  current,  rather 
than  new,  position.  One  effect  of  this  is  that  changes  may  take  more  iterations  to  propagate  down  the  chain. 
Another  effect  is  that  certain  instabilities  can  occur.  !-or  example,  a  zig-zagging  path  can  tlip  from  the  original 
path  to  its  mirror  image  on  alternate  iterations.  This  can  be  fixed  with  appropriate  damping. 
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6.  Remaining  Work 


Path  Relaxation  would  be  easy  to  extend  to  higher  dimensions.  It  could  be  used,  for  example,  for  a 
three-dimensional  search  to  be  used  by  underwater  vehicles  maneuvering  through  a  drilling  platform. 
Another  use  for  higher-dimensional  searches  would  be  to  include  rotations  for  asymmetric  vehicles.  Yet 
another  application  would  be  to  model  moving  obstacles;  then  the  third  dimension  becomes  time,  with  the 
cost  of  a  grid  point  having  to  do  with  distance  to  all  objects  at  that  time.  This  would  have  a  slightly  different 
flavor  than  the  other  higher-dimensional  extensions:  it  is  possible  to  go  both  directions  in  x,  y,  /.  and  theta, 
but  only  one  direction  in  the  time  dimension. 

Another  possible  extension  has  to  do  with  smoothing  out  sharp  corners.  All  wheels  on  die  CMU  Rover 
steer,  so  it  can  follow  a  path  with  sharp  corners  if  necessary.  Many  other  vehicles  arc  not  so  maneuverable; 
they  may  steer  like  a  car.  with  a  minimum  possible  turning  radius.  In  order  to  accommodate  those  vehicles,  it 
would  be  necessary  to  restrict  both  the  graph  search  and  relaxation  steps.  A  related  problem  is  to  use  a 
smoothly  curved  path  rather  than  a  series  of  linear  segments. 

Occlusions  could  be  taken  into  account  in  fading  out  objects.  The  current  rules  decrease  the  weight  of  an 
object  if  it  has  not  been  seen  on  the  current  step,  but  is  within  the  field  of  view  of  the  camera.  Sometimes  an 
object  may  not  be  seen  because  it  is  behind  a  nearer  object.  So  a  better  fading  algorithm  would  take  into 
account  nearby  objects  that  may  be  blocking  further  points  from  view.  This  strategy  becomes  much  more 
interesting  with  a  more  sophisticated  vision  system,  for  example  one  that  reports  surfaces  rather  than  points. 

In  the  current  system,  die  error  function  is  the  same  for  each  object.  One  possible  extension  is  to  increase 
die  size  of  obstacles  further  away  from  die  start  position,  reflecting  the  increased  positional  error  of  die 
vehicle  as  it  moves.  This  has  not  been  done  for  the  l-ido/ Rover  system,  since  a  typical  move  will  probably  be 
less  than  a  meter  lung.  Hut  other  systems,  interested  in  longer  moves,  may  want  to  increase  their  margin  of 
error  along  the  padi.  Another  possibility  would  be  to  use  a  constant  error  function,  but  to  determine  the 
distance  to  be  traveled  in  one  step  by  seeing  how  far  along  the  current  path  the  vehicle  can  move  before  its 
worst-case  position  error  exceeds  the  clearance  available. 

Another  area  that  will  require  more  work  is  time  vs.  accuracy  trade-offs.  Some  algorithms  have  a  time 
complexity  that  depends  only  on  the  number  of  objects  or  the  number  of  object  vertices.  The  run-time  of 
path  relaxation,  in  contrast,  depends  on  the  size  of  the  area  to  be  covered  and  on  the  grid  spacing.  So 
situations  with  lots  of  empty  space  and  only  a  few  large  objects  favor  other  algorithms.  Part  of  that  can  be 
overcome  with  a  strategy  that  automatically  uses  a  larger  grid  size  in  clear  areas.  I  bis  brings  up  problems  of 
representation  of  non-homogcncous  grids  and  of  deciding  when  to  switch  grid  size.  Neither  of  these  appears 
to  be  theoretically  difficult,  but  both  will  take  some  thought  to  do  elegantly. 

A  related  problem  is  use  of  padi  relaxation  for  paths  with  tight  clearances.  I  f  it  is  really  necessary  to  squeeze 
between  two  obstacles,  the  current  algorithm  may  not  provide  a  detailed  enough  path.  If  multiple  grid  sizes 
arc  implemented,  it  will  be  possible  to  get  smaller,  as  well  as  larger  spacings.  and  to  find  good  paths  even  in 
tight  spots. 


Path  relaxation,  as  well  as  almost  all  existing  path  planners,  deals  only  with  geometric  information.  A  large 
part  of  a  robot's  world  knowledge,  however,  may  be  in  partially  symbolic  form,  l  or  example,  a  map 
assembled  by  the  vehicle  itself  may  have  very  precise  local  patches,  each  measured  from  one  robot  location. 
The  relations  between  patches,  though,  will  probably  be  much  less  precise,  since  they  depend  on  robot 
motion  from  one  step  to  the  next.  Using  such  a  mixture  of  constraints  is  a  hard  problem. 
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Example  Runs 


Figures  5,  6,  and  7  form  a  set.  Figure  5  is  a  run  from  scratch,  using  real  data  extracted  from  the  stored 
images  by  die  Fido  vision  and  navigation  system.  Objects  arc  represented  as  little  circles,  where  the  si/e  of  the 
circle  is  the  positional  uncertainly  of  the  stereo  system.  I'hc  numbers  are  not  all  consecutive,  because  some  of 
the  points  being  tracked  arc  on  the  floor  or  arc  high  off  the  ground,  and  therefore  aren't  obstacles.  The  dotted 
lines  surround  the  area  not  in  the  Held  of  view;  this  should  extend  to  negative  infinity.  The  start  position  of 
the  robot  is  approximately  (0.  -.2)  and  die  goal  is  (0,  14.5).  The  grid  path  found  is  marked  by  0’s.  After  one 
iteration  of  relaxation,  the  path  is  marked  by  l's.  After  the  second  relaxation,  the  path  is  marked  by  2’s.  I'hc 
greatest  change  from  l  to  2  was  less  titan  .3  meters,  die  direshold,  so  the  process  stopped.  The  size  of  the 
"hills"  in  die  cost  function  is  1  meter,  which  means  that  the  robot  w  ill  try  to  stay  I  meter  away  from  obstacles 
unless  that  causes  it  to  go  too  far  out  of  its  way. 

After  the  robot’s  first  move,  it  tries  to  reuse  the  existing  path.  The  robot  is  now  at  about  (0.1,  0.8).  It  deletes 
die  old  start  node  and  the  nodes  at  (0,0)  and  (0,1),  and  links  its  current  position  into  the  path.  In  diis  case  it 
had  no  new  objects  to  add,  so  the  path  was  still  passable.  It  then  uses  the  old  path  without  having  to  plan 
from  scratch. 

After  die  next  step,  there  arc  some  new  objects  added.  In  particular,  object  #61  is  too  close  to  the  old  path, 
so  a  new  one  has  to  be  planned.  Again,  die  0’s  arc  the  grid  path,  and  the  l’s  and  2’s  mark  steps  of  the 
relaxation  algorithm. 

figures  8,  9.  and  10  arc  the  same  run  as  shown  in  figures  5.  6,  and  7,  except  that  the  relaxation  step  is 
simulated  to  be  in  parallel.  The  biggest  difference  is  near  die  end  of  the  path  in  figure  8,  where  the  path  is 
somewhat  jagged.  The  path  Hips  between  one  state  on  even-numbered  iterations  (0  and  2)  and  the  opposite 
state  on  odd-numbered  steps  (1).  This  could  be  fixed  with  damping,  at  die  expense  of  slower  convergence  on 
other  parts  of  die  padi. 
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